#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdint.h"
#include <stdarg.h>
#include <math.h>

// RT-Thread 头文件
#include <rtthread.h>
#include <rthw.h>
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include <rtdevice.h>
#include <board.h>

#include "rt_thread_demo.h"
#include "../hardware/PWM/fly_pwm.h"                    // PWM
#include "../hardware/JY61P/JY61P.h"                    // JY61P
#include "../hardware/Flight_control/fly_control.h"     // 飞控
#include "../hardware/Flight_control/pid.h"             // PID控制
#include "../applications/De_bug_mouse/de_bug_mouse.h"  // 相关辅助函数

// HAL库头文件
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "rt_thread_demo.h"
#include "../hardware/PWM/fly_pwm.h"             // PWM



#define Motor_min_limit 1000
#define Motor_max_limit 2000
#define Control_updata_num 10
#define Speed_Base 1000
#define AngletoGyro 50


float Speed_base = Speed_Base;
float wa_out,wd_out,sa_out,sd_out;
float wa_base = Speed_Base,wd_base = Speed_Base,sa_base = Speed_Base,sd_base = Speed_Base;

PID_t height_t={
	.target = 0,
	.actual = &pitch,
	
	.kp = 0, 
	.ki = 0,
	.kd = 0,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = &accz,
	
	.limit_out = 0,
	.limit_Int = 0,
	.out = 0,
};

PID_t pitch_t={
	.target = 0.58,
	.actual = &pitch,
	
	.kp = 1.5,
	.ki = 0.002,
	.kd = 0.5,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = &gyrox,
	
	.limit_out = AngletoGyro,
	.limit_Int = 1000, 
	.out = 0,
};

PID_t roll_t={
	.target = 0,
	.actual = &roll,
	
	.kp = 1.5,
	.ki = 0.001,
	.kd = 0.5,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = &gyroy,
	
	.limit_out = AngletoGyro,
	.limit_Int = 1000,
	.out = 0,
};

PID_t yaw_t={
	.target = 0,
	.actual = &yaw,
	
	.kp = 0,
	.ki = 0,
	.kd = 0,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = &gyroz,
	
	.limit_out = 5,
	.limit_Int = 0,
	.out = 0,
};


PID_t gyrox_t={
	.target = 0,
	.actual = &gyrox,
	
	.kp = 0,			//3 1.6 
	.ki = 0,
	.kd = 0,			//	10
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = NULL,
	
	.limit_out = 1000,
	.limit_Int = 0,
	.out = 0,
};


PID_t gyroy_t={
	.target = 0,
	.actual = &gyroy,
	
	.kp = 2.5,
	.ki = 0,
	.kd = 10,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = NULL,
	
	.limit_out = 1000,
	.limit_Int = 0,
	.out = 0,
};


PID_t gyroz_t={
	.target = 0,
	.actual = &gyroz,
	
	.kp = 2,
	.ki = 0,
	.kd = 0,
	
	.error = 0,
	.errInt = 0,
	.err_last = 0,
	.fferr_t = NULL,
	
	.limit_out = 1000,
	.limit_Int = 0,
	.out = 0,
};

void PID_Param_LowSet(void)
{
	pitch_t.kp = 1.5;
	pitch_t.ki = 0.001;
	pitch_t.kd = 0.5;
	
	roll_t.kp = 1.5;
	roll_t.ki = 0.001;
	roll_t.kd = 0.5;
	
	yaw_t.kp = 1;
	yaw_t.ki = 0;
	yaw_t.kd = 0;
	
	gyrox_t.kp = 2.5;
	gyrox_t.ki = 0;
	gyrox_t.kd = 10;
	
	gyroy_t.kp = 2.5;
	gyroy_t.ki = 0;
	gyroy_t.kd = 10;
	
	gyroz_t.kp = 1.5;
	gyroz_t.ki = 0;
	gyroz_t.kd = 0;
	
}

void PID_Param_HighSet(void)
{
	double kk = 0.55;
	
	pitch_t.kp = 1.5 * kk;
	pitch_t.ki = 0.001 ;
	pitch_t.kd = 0.5 * kk;
	
	roll_t.kp = 1.5 * kk;
	roll_t.ki = 0.001 ;
	roll_t.kd = 0.5 * kk;
	
	yaw_t.kp = 0.5 * kk;
	yaw_t.ki = 0 * kk;
	yaw_t.kd = 0 * kk;
	
	gyrox_t.kp = 1.7 * kk;
	gyrox_t.ki = 0 * kk;
	gyrox_t.kd = 3.0 * kk;
	
	gyroy_t.kp = 1.7 * kk;
	gyroy_t.ki = 0 * kk;
	gyroy_t.kd = 3.0 * kk;
	
	gyroz_t.kp = 0.5 * kk;
	gyroz_t.ki = 0 * kk;
	gyroz_t.kd = 0 * kk;
}

void PID_gyro_target_set(float gyrox,float gyroy,float gyroz)
{
	gyrox_t.target = gyrox;
	gyroy_t.target = gyroy;
	gyroz_t.target = gyroz;
}

void PID_angle_target_set(float pitch,float roll,float yaw)
{
	pitch_t.target = pitch;
	roll_t.target = roll;
	yaw_t.target = yaw;
}

void PID_Clear(void)
{
	pitch_t.errInt = 0;
	roll_t.errInt = 0;
	yaw_t.errInt = 0;
	
	gyrox_t.errInt = 0;
	gyroy_t.errInt = 0;
	gyroz_t.errInt = 0;
	
	//p->err_last = 0;
}

void PID_Updata(PID_t* p)
{
	p->err_last = 0.4*p->err_last + 0.6*p->error;
	p->error = p->target - *p->actual;
	
	if (p->ki != 0)
	{
		p->errInt += p->error;
	}
	else
	{
		p->errInt = 0;
	}
	
	if (p->errInt > p->limit_Int) {p->errInt = p->limit_Int;}
	if (p->errInt < -p->limit_Int) {p->errInt = -p->limit_Int;}
	
	if(p->fferr_t == NULL)
	{
		p->out = p->kp * p->error
			   + p->ki * p->errInt
			   + p->kd * (p->error - p->err_last);
	}
	else
	{
		p->out = p->kp * p->error
			   + p->ki * p->errInt
			   + p->kd * *(p->fferr_t);
	}
	
	if (p->out > p->limit_out) 	{p->out = p->limit_out;}
	if (p->out < -p->limit_out) {p->out = -p->limit_out;}
}

/*
 *	@brief 这是电机设置函数（电调PWM协议）
 *
 *	@param 如图为电机四轴上的旋转大小
 *			wa	4	wd  2
 *			
 *			sa	3	sd  1
 *
**/
void Motor_Set(float wa, float wd, float sa, float sd)
{
//    rt_kprintf("%d\n",(int)wa);
	__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_4,(uint32_t)wa);
	__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_2,(uint32_t)wd);
	__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_3,(uint32_t)sa);
	__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_1,(uint32_t)sd);
}

void Motor_limit(float* yuanshi)
{
	if(*yuanshi > Motor_max_limit) *yuanshi = Motor_max_limit;
	else if(*yuanshi < Motor_min_limit) *yuanshi = Motor_min_limit;
}

void Control_Init(void)
{
	Motor_Set(1999,1999,1999,1999);
	HAL_Delay(8000);
	Motor_Set(1001,1001,1001,1001);
	HAL_Delay(5000);
//	PID_Clear(&pitch_t);
//	PID_Clear(&roll_t);
//	PID_Clear(&yaw_t);
//	PID_Clear(&height_t);
//	Motor_Set(1500,1500,1500,1500);
//	HAL_Delay(8000);
	Motor_Set(1001,1001,1001,1001);
	
}

uint8_t Control_updata(void)
{
	static uint8_t count = 0;
	static float wa_sum,wd_sum,sa_sum,sd_sum;
	if(count < Control_updata_num)
	{
		wa_sum = wa_out;
		wd_sum = wd_out;
		sa_sum = sa_out;
		sd_sum = sd_out;
		return 0;
	}
	else
	{
		count = 0;
		//set motor speed_base
		wa_base = wa_sum / Control_updata_num;
		wd_base = wd_sum / Control_updata_num;
		sa_base = sa_sum / Control_updata_num;
		sd_base = sd_sum / Control_updata_num;
		//Init PID
//		PID_Clear(&pitch_t);
//		PID_Clear(&roll_t);
//		PID_Clear(&yaw_t);
//		PID_Clear(&height_t);
		return 1;
	}
}

void Control_Angle(void)
{
//	if(wa_base < 1400)
//	{
//		PID_Clear(&pitch_t);
//		PID_Clear(&roll_t);
//	}
	//PID updata
	//PID_Updata(&height_t);
	//height_t.out = 0;
	PID_Updata(&pitch_t);
	PID_Updata(&roll_t);
	PID_Updata(&yaw_t);
	PID_gyro_target_set(pitch_t.out,roll_t.out,yaw_t.out);
}

void Control_Gyro(void)
{
	
	PID_Updata(&gyrox_t);
	PID_Updata(&gyroy_t);
	 PID_Updata(&gyroz_t);
	
	//updata motor speed
//	wa_out = wa_base + height_t.out-pitch_t.out+roll_t.out+yaw_t.out;
//	wd_out = wd_base + height_t.out-pitch_t.out-roll_t.out-yaw_t.out;
//	sa_out = sa_base + height_t.out+pitch_t.out+roll_t.out-yaw_t.out;
//	sd_out = sd_base + height_t.out+pitch_t.out-roll_t.out+yaw_t.out;
	
	//Print(&huart4,"%.2f,%.2f\r\n",gyrox_t.out,wa_base);
	wa_out = wa_base - gyroy_t.out + gyrox_t.out + gyroz_t.out;
	wd_out = wd_base - gyroy_t.out - gyrox_t.out - gyroz_t.out;
	sa_out = sa_base + gyroy_t.out + gyrox_t.out - gyroz_t.out;
	sd_out = sd_base + gyroy_t.out - gyrox_t.out + gyroz_t.out;
	
//	static int temp = 0;
//	if(temp == 100)
//	  {
//			Print(&huart4,"%.0f,%.0f,%.0f,%.0f,%.0f\r\n",wa_out,wd_out,sa_out,sd_out,wa_base);
//			temp = 0;
//	  }
//	  temp++;
	
	
	//Print(&huart4,"%.0f\r\n",wa_out);
	//limit motor speed
	Motor_limit(&wa_out);
	Motor_limit(&wd_out);
	Motor_limit(&sa_out);
	Motor_limit(&sd_out);
	
	//set motor speed
	Motor_Set(wa_out, wd_out, sa_out, sd_out);
}

void Control(void)
{
	PID_Updata(&gyrox_t);
	//PID_Updata(&gyroy_t);
	//PID_Updata(&gyroz_t);
	
	//updata motor speed
//	wa_out = wa_base + height_t.out-pitch_t.out+roll_t.out+yaw_t.out;
//	wd_out = wd_base + height_t.out-pitch_t.out-roll_t.out-yaw_t.out;
//	sa_out = sa_base + height_t.out+pitch_t.out+roll_t.out-yaw_t.out;
//	sd_out = sd_base + height_t.out+pitch_t.out-roll_t.out+yaw_t.out;
	
	//Print(&huart4,"%.2f,%.2f\r\n",gyrox_t.out,wa_base);
	wa_out = wa_base - gyroy_t.out + gyrox_t.out + gyroz_t.out;
	wd_out = wd_base - gyroy_t.out - gyrox_t.out - gyroz_t.out;
	sa_out = sa_base + gyroy_t.out + gyrox_t.out - gyroz_t.out;
	sd_out = sd_base + gyroy_t.out - gyrox_t.out + gyroz_t.out;
	//Print(&huart4,"%.0f\r\n",wa_out);
	//limit motor speed
	Motor_limit(&wa_out);
	Motor_limit(&wd_out);
	Motor_limit(&sa_out);
	Motor_limit(&sd_out);
	
	//set motor speed
	Motor_Set(wa_out, wd_out, sa_out, sd_out);

}



